#include "CVehicleEntity.hpp"


CVehicleEntity::~CVehicleEntity()
{
	//
}

void CVehicleEntity::postConstruction()
{
	this->sceneNodeRoot		= CAplicacion::getSingleton()->getSceneManager()->getRootSceneNode()->createChildSceneNode(id);
	this->sceneNodeRoot->translate(position.x, position.y, position.z);
	this->physicsActor		= NULL;
}

void CVehicleEntity::postAddChild()
{
	physicsActor->userData = (void *)this;
}

CVehicleEntity::CVehicleEntity
(
	CarType carType, 
	String	id,
	NxVec3	position,
	NxReal	axisYOrientation
)
{
	switch(carType)
	{
		case SPORTCAR :
		{
			//---------------------------------------------------------------------------

			NxVehicleMotorDesc motorConfiguration;					// Corvettte configuration
			motorConfiguration.torqueCurve.insert(1000.f, 393.f);	// rotation-torque pair (RPM vs Nm)
			motorConfiguration.torqueCurve.insert(2000.f, 434.f);	// rotation-torque pair (RPM vs Nm)
			motorConfiguration.torqueCurve.insert(4000.f, 475.f);	// rotation-torque pair (RPM vs Nm)
			motorConfiguration.torqueCurve.insert(5000.f, 475.f);	// rotation-torque pair (RPM vs Nm)
			motorConfiguration.torqueCurve.insert(6000.f, 366.f);	// rotation-torque pair (RPM vs Nm)
			motorConfiguration.minRpmToGearDown	= 2500.f;			// For tiptronic control
			motorConfiguration.maxRpmToGearUp	= 5000.f;			// For tiptronic control
			motorConfiguration.minRpm			= 1500.f;			// For tacometer control
			motorConfiguration.maxRpm			= 6000.f;			// For tacometer control

			NxVehicleGearDesc gearsConfiguration;					// Corvettte configuration
			gearsConfiguration.nbForwardGears		=  6;			// 6 forward and 1 backward gears
			gearsConfiguration.forwardGearRatios[0] =  2.66f;		// 1st forward gear ratio
			gearsConfiguration.forwardGearRatios[1] =  1.78f;		// 2nd forward gear ratio
			gearsConfiguration.forwardGearRatios[2] =  1.30f;		// 3th forward gear ratio
			gearsConfiguration.forwardGearRatios[3] =  1.09f;		// 4th forward gear ratio
			gearsConfiguration.forwardGearRatios[4] =  0.74f;		// 5th forward gear ratio
			gearsConfiguration.forwardGearRatios[5] =  0.50f;		// 6th forward gear ratio
			gearsConfiguration.backwardGearRatio	= -2.90f;		// 1st backward gear ratio

			new CVehicleEntity
			(
				id,
				position,						// position 
				axisYOrientation,				// axisYOrientation
				1200,							// mass	(Kg)						| 2400.0f MonsterTruck | 1200.0f SportCar
				NxVec3(0.0f,-1.0f,0.0f),		// centerOfMass						| -2.0f   MonsterTruck | -0.5f   SportCar
				80.f,							// maxVelocity (Km/h)				| 40.0f   MonsterTruck | 80.0f   SportCar
				motorConfiguration,				// Torque curve by RPM-Nm pairs		| As a Corvette
				gearsConfiguration,				// Forward and backward gear ratios	| As a Corvette
				33.42f,							// differentialRatio				| 20.5f   MonsterTruck | 3.42f   SportCar
				true,							// useFrontWheelDrive				| true    MonsterTruck | true    SportCar
				true,							// useBackWheelDrive (to get 4x4 )	| true    MonsterTruck | false   SportCar
				4.0f,							// chassisLengthX					| 5.0f
				1.0f,							// chassisLengthY					| 0.8f
				2.0f,							// chassisLengthZ					| 2.4f
				1.0f,							// frontAxisWheelsDelta				| 1.6f
				0.5f,							// upAxisWheelsDelta				| 0.4f
				1.0f,							// lateralAxisWheelsDelta			| 1.4f    MonsterTruck | 1.09f   SportCar
				10.0f,							// wheelApproximation (iterations)	| 10.0f
				0.5f,							// wheelRadius						| 1.2f    MonsterTruck | 0.33f   SportCar
				0.2f,							// wheelSuspension					| 0.6f    MonsterTruck | 0.2f    SportCar
				7000.0f,						// springRestitution				| 4000.0f MonsterTruck | 7000.0f SportCar
				800.0f,							// springDamping					| 800.0f
				0.0f,							// springBias						| 0.0f
				1.0f,							// maxBrakeForce					| 0.5f    MonsterTruck | 1.0f    SportCar
				NxVec3(1.8f, 0.0f, 0.0f),		// steeringSteerPoint				| ( 1.8f, 0.0f, 0.0f)
				NxVec3(-1.5f, 0.0f, 0.0f),		// steeringTurnPoint				| (-1.5f, 0.0f, 0.0f)
				0.04f,							// digitalSteeringDelta				| 0.04f
				30.f							// steeringMaxAngle					| 30.0f
			);

			//---------------------------------------------------------------------------

			break;
		}
		case MONSTERTRUCK :
		{
			//---------------------------------------------------------------------------

			NxVehicleMotorDesc motorConfiguration;					// Corvettte configuration
			motorConfiguration.torqueCurve.insert(1000.f, 393.f);	// rotation-torque pair (RPM vs Nm)
			motorConfiguration.torqueCurve.insert(2000.f, 434.f);	// rotation-torque pair (RPM vs Nm)
			motorConfiguration.torqueCurve.insert(4000.f, 475.f);	// rotation-torque pair (RPM vs Nm)
			motorConfiguration.torqueCurve.insert(5000.f, 475.f);	// rotation-torque pair (RPM vs Nm)
			motorConfiguration.torqueCurve.insert(6000.f, 366.f);	// rotation-torque pair (RPM vs Nm)
			motorConfiguration.minRpmToGearDown	= 2500.f;			// For tiptronic control
			motorConfiguration.maxRpmToGearUp	= 5000.f;			// For tiptronic control
			motorConfiguration.minRpm			= 1500.f;			// For tacometer control
			motorConfiguration.maxRpm			= 6000.f;			// For tacometer control

			NxVehicleGearDesc gearsConfiguration;					// Corvettte configuration
			gearsConfiguration.nbForwardGears		=  6;			// 6 forward and 1 backward gears
			gearsConfiguration.forwardGearRatios[0] =  2.66f;		// 1st forward gear ratio
			gearsConfiguration.forwardGearRatios[1] =  1.78f;		// 2nd forward gear ratio
			gearsConfiguration.forwardGearRatios[2] =  1.30f;		// 3th forward gear ratio
			gearsConfiguration.forwardGearRatios[3] =  1.09f;		// 4th forward gear ratio
			gearsConfiguration.forwardGearRatios[4] =  0.74f;		// 5th forward gear ratio
			gearsConfiguration.forwardGearRatios[5] =  0.50f;		// 6th forward gear ratio
			gearsConfiguration.backwardGearRatio	= -2.90f;		// 1st backward gear ratio

			new CVehicleEntity
			(
				id,
				NxVec3(0.0f, 2.0f, 0.0f),		// position 
				180.0f,							// axisYOrientation
				2400,							// mass	(Kg)						| 2400.0f MonsterTruck | 1200.0f SportCar
				NxVec3(0.0f,-2.0f,0.0f),		// centerOfMass						| -2.0f   MonsterTruck | -0.5f   SportCar
				40.f,							// maxVelocity (Km/h)				| 40.0f   MonsterTruck | 80.0f   SportCar
				motorConfiguration,				// Torque curve by RPM-Nm pairs		| As a Corvette
				gearsConfiguration,				// Forward and backward gear ratios	| As a Corvette
				20.5f,							// differentialRatio				| 20.5f   MonsterTruck | 3.42f   SportCar
				true,							// useFrontWheelDrive				| true    MonsterTruck | true    SportCar
				true,							// useBackWheelDrive (to get 4x4 )	| true    MonsterTruck | false   SportCar
				5.0f,							// chassisLengthX					| 5.0f
				0.8f,							// chassisLengthY					| 0.8f
				2.4f,							// chassisLengthZ					| 2.4f
				1.6f,							// frontAxisWheelsDelta				| 1.6f
				0.4f,							// upAxisWheelsDelta				| 0.4f
				1.4f,							// lateralAxisWheelsDelta			| 1.4f    MonsterTruck | 1.09f   SportCar
				10.0f,							// wheelApproximation (iterations)	| 10.0f
				1.2f,							// wheelRadius						| 1.2f    MonsterTruck | 0.33f   SportCar
				0.6f,							// wheelSuspension					| 0.6f    MonsterTruck | 0.2f    SportCar
				4000.0f,						// springRestitution				| 4000.0f MonsterTruck | 7000.0f SportCar
				800.0f,							// springDamping					| 800.0f
				0.0f,							// springBias						| 0.0f
				0.5f,							// maxBrakeForce					| 0.5f    MonsterTruck | 1.0f    SportCar
				NxVec3(1.8f, 0.0f, 0.0f),		// steeringSteerPoint				| ( 1.8f, 0.0f, 0.0f)
				NxVec3(-1.5f, 0.0f, 0.0f),		// steeringTurnPoint				| (-1.5f, 0.0f, 0.0f)
				0.04f,							// digitalSteeringDelta				| 0.04f
				30.f							// steeringMaxAngle					| 30.0f
			);

			//---------------------------------------------------------------------------

			break;
		}
		default :
		{
			break;
		}
	}
}

CVehicleEntity::CVehicleEntity
(
	String				id,
	NxVec3				position,	
	NxReal				axisYOrientation,
	NxReal				mass,
	NxVec3				centerOfMass,
	NxReal				maxVelocity,
	NxVehicleMotorDesc	motorConfiguration,
	NxVehicleGearDesc	gearsConfiguration,
	NxReal				differentialRatio,
	bool				useFrontWheelDrive,
	bool				useBackWheelDrive,
	NxReal				chassisLengthX,
	NxReal				chassisLengthY,
	NxReal				chassisLengthZ,
	NxReal				frontAxisWheelsDelta,
	NxReal				upAxisWheelsDelta,
	NxReal				lateralAxisWheelsDelta,
	NxReal				wheelApproximation,
	NxReal				wheelRadius,
	NxReal				wheelSuspension,
	NxReal				springRestitution,
	NxReal				springDamping,
	NxReal				springBias,
	NxReal				maxBrakeForce,
	NxVec3				steeringSteerPoint,
	NxVec3				steeringTurnPoint,
	NxReal				digitalSteeringDelta,
	NxReal				steeringMaxAngle
)
{
	this->id = id;
	this->position = position;
	
	postConstruction();

	//---------------------------------------------------------
	
	NxVehicleDesc vehicleDesc;

	// Set shapes (A car towards x positive axis)

	NxBoxShapeDesc boxShapes[2];
	boxShapes[0].dimensions.set(chassisLengthX/2.0f, chassisLengthY/2.0f, chassisLengthZ/2.0f);
	boxShapes[1].dimensions.set(chassisLengthX/4.0f, chassisLengthY/2.0f, chassisLengthZ/2.0f);
	boxShapes[1].localPose.t.set(0.0f, chassisLengthY/2.0f, 0.0f);
	vehicleDesc.carShapes.pushBack(&boxShapes[0]);
	vehicleDesc.carShapes.pushBack(&boxShapes[1]);

	// Set motor configuration

	vehicleDesc.motorDesc = &motorConfiguration;

	// Set gears configuration

	vehicleDesc.gearDesc = &gearsConfiguration;

	vehicleDesc.differentialRatio = differentialRatio;

	// Set the wheels description

	NxWheelDesc wheelDesc[4];
	
	for(NxU32 i=0;i<4;i++)
	{
		wheelDesc[i].wheelApproximation = wheelApproximation;
		wheelDesc[i].wheelRadius		= wheelRadius;
		wheelDesc[i].wheelSuspension	= wheelSuspension;
		wheelDesc[i].springRestitution	= springRestitution;
		wheelDesc[i].springDamping		= springDamping;
		wheelDesc[i].springBias			= springBias;
		wheelDesc[i].maxBrakeForce		= maxBrakeForce;

		vehicleDesc.carWheels.pushBack(&wheelDesc[i]);
	}

	// Delta offsets from center

	wheelDesc[0].position.set( frontAxisWheelsDelta,-upAxisWheelsDelta, lateralAxisWheelsDelta);// Front left  wheel
	wheelDesc[1].position.set( frontAxisWheelsDelta,-upAxisWheelsDelta,-lateralAxisWheelsDelta);// Front right wheel
	wheelDesc[2].position.set(-frontAxisWheelsDelta,-upAxisWheelsDelta, lateralAxisWheelsDelta);// Back  left  wheel
	wheelDesc[3].position.set(-frontAxisWheelsDelta,-upAxisWheelsDelta,-lateralAxisWheelsDelta);// Back  right wheel

	wheelDesc[0].wheelFlags |= (useFrontWheelDrive?NX_WF_ACCELERATED:0) | NX_WF_STEERABLE_INPUT;
	wheelDesc[1].wheelFlags |= (useFrontWheelDrive?NX_WF_ACCELERATED:0) | NX_WF_STEERABLE_INPUT;
	wheelDesc[2].wheelFlags |= (useBackWheelDrive?NX_WF_ACCELERATED:0)  | NX_WF_AFFECTED_BY_HANDBRAKE;
	wheelDesc[3].wheelFlags |= (useBackWheelDrive?NX_WF_ACCELERATED:0)  | NX_WF_AFFECTED_BY_HANDBRAKE;

	// Set misc properties

	vehicleDesc.position				= position;
	vehicleDesc.mass					= mass;
	vehicleDesc.maxVelocity				= maxVelocity;
	vehicleDesc.centerOfMass			= centerOfMass;
	vehicleDesc.steeringSteerPoint		= steeringSteerPoint;
	vehicleDesc.steeringTurnPoint		= steeringTurnPoint;
	vehicleDesc.digitalSteeringDelta	= digitalSteeringDelta;
	vehicleDesc.steeringMaxAngle		= steeringMaxAngle;

	vehicle = NxVehicle::createVehicle
	(
		CAplicacion::getSingleton()->getNxScene(), 
		&vehicleDesc
	);

	physicsActor = vehicle->getActor();

	/*
	NxQuat q;
	q.fromAngleAxis(axisYOrientation, NxVec3(0.0f, 1.0f, 0.0f));
	vehicle->getActor()->setGlobalOrientationQuat(q);
	*/

	postAddChild();
	
	//------------------------------------------------------------

	NxShape*const* shapes = physicsActor->getShapes();
	
	NxU32 nShapes = physicsActor->getNbShapes();

	while(nShapes--)
	{
		if (physicsActor->isDynamic())
		{
			NxShape* shape = shapes[nShapes];

			String name = id+"_"+nShapes;

			switch(shape->getType())
			{
				case NX_SHAPE_BOX:
				{
					//----------------------------------------------------

					NxVec3	shapePosition		= shape->getGlobalPosition() + (shape->getLocalPose().t);
					NxVec3	shapeDimension		= (shape->isBox()->getDimensions()*2.0f)/100.0f;
					NxMat33 shapeOrientation	= shape->getGlobalOrientation();

					NxQuat q;
					shapeOrientation.toQuat(q);

					Entity* entity = CAplicacion::getSingleton()->getSceneManager()->createEntity
					(
						name, 
						CAplicacion::getSingleton()->getSceneManager()->PT_CUBE
					);
					entity->setMaterialName("Examples/Car");
					entity->setCastShadows( true );

					SceneNode* sceneNode = sceneNodeRoot->createChildSceneNode(name);
					sceneNode->setPosition(CUtiles::toVector3(shapePosition));
					sceneNode->setScale(CUtiles::toVector3(shapeDimension));
					sceneNode->setOrientation( CUtiles::toQuaternion(q));
					sceneNode->attachObject( entity );

					shape->userData = sceneNode; // !!!!!!

					//----------------------------------------------------
					break;
				}
				case NX_SHAPE_WHEEL:
				{
					//----------------------------------------------------

					if(shape->is(NX_SHAPE_WHEEL) != NULL)
					{
						NxWheelShape* wheelShape = (NxWheelShape*)shape;

						NxVec3 shapePosition		= shape->getGlobalPosition(); // + shape->getLocalPosition();
						//NxMat33 shapeOrientation	= shape->getGlobalOrientation();

						NxQuat shapeOrientation(shape->getGlobalPose().M);

						//NxQuat q; shapeOrientation.toQuat(q);

						float r = wheelShape->getRadius();
						//float a = wheelShape->getSteerAngle();
						
						Entity* entity = CAplicacion::getSingleton()->getSceneManager()->createEntity
						(
							name,  
							"Barrel.mesh"
						);
						entity->setCastShadows( true );

						SceneNode* sceneNode = sceneNodeRoot->createChildSceneNode(name);
						
						Quaternion* qq = new Quaternion
						(
							Radian(NxMath::degToRad(90.0f)),
							Vector3(1,0,0)
						);

						sceneNode->setPosition(CUtiles::toVector3(shapePosition));
						sceneNode->setScale(r/2.5,r/8,r/2.5);
						//sceneNode->setOrientation(*qq);
						sceneNode->setOrientation( CUtiles::toQuaternion(shapeOrientation));

						//sceneNode->translate(-r/2,0,0);
						//sceneNode->rotate(Vector3(0,1,0),Radian(NxMath::degToRad(a)));
						//sceneNode->setOrientation( CUtiles::toQuaternion(q));
						
						sceneNode->attachObject( entity );

						shape->userData = sceneNode; // !!!!!!
					}
					
					//----------------------------------------------------
					break;
				}
				default:
				{
					break;
				}
			}
		}
	}

	//------------------------------------------------------------

	//wheelAxleRotationSum = 0;

}

void CVehicleEntity::refrescar(Real timeSinceLastFrame)
{
	//PhysX le dice a OGRE la posicion y orientacion  de la entidad
	//getSceneNodeRoot()->setPosition(CUtiles::toVector3(physicsActor->getGlobalPosition()));
	//getSceneNodeRoot()->setOrientation(CUtiles::toQuaternion(physicsActor->getGlobalOrientationQuat()));

	//------------------------------------------------------------

	if(CAplicacion::getSingleton()->wiimoteController->isConnected())
	{
		CAplicacion::getSingleton()->wiimoteController->wiimoteDriver.RefreshState();

		if(CAplicacion::getSingleton()->wiimoteController->getButtonLeftStatus())
		{
			CUtiles::setSteering(-1);
		}

		if(CAplicacion::getSingleton()->wiimoteController->getButtonRightStatus())
		{
			CUtiles::setSteering(1);
		}

		if(CAplicacion::getSingleton()->wiimoteController->getButtonUpStatus())
		{
			CUtiles::setAcceleration(1);
		}

		if(CAplicacion::getSingleton()->wiimoteController->getButtonDownStatus())
		{
			CUtiles::setAcceleration(-1);
		}
	}
	
	vehicle->control(CUtiles::getSteering(), false, CUtiles::getAcceleration(), false, false);
	vehicle->updateVehicle(timeSinceLastFrame);
	CUtiles::setSteering(0);
	CUtiles::setAcceleration(0);

	//------------------------------------------------------------

	NxShape*const* shapes = physicsActor->getShapes();
	
	NxU32 nShapes = physicsActor->getNbShapes();

	while(nShapes--)
	{
		if (physicsActor->isDynamic())
		{
			NxShape* shape = shapes[nShapes];

			String name = id+"_"+nShapes;

			switch(shape->getType())
			{
				case NX_SHAPE_BOX:
				{
					//----------------------------------------------------

					NxVec3	shapePosition		= shape->getGlobalPosition() + shape->getLocalPosition();
					NxMat33 shapeOrientation	= shape->getGlobalOrientation();

					NxQuat q;
					shapeOrientation.toQuat(q);

					SceneNode* sceneNode = (SceneNode*)shape->userData;

					sceneNode->setPosition(CUtiles::toVector3(shapePosition));
					sceneNode->setOrientation( CUtiles::toQuaternion(q));
					
					//----------------------------------------------------
					
					break;
				}
				case NX_SHAPE_WHEEL:
				{
					//----------------------------------------------------
					if(shape->is(NX_SHAPE_WHEEL) != NULL)
					{
						NxWheelShape* wheelShape = (NxWheelShape*)shape;

						NxVec3 shapePosition = shape->getGlobalPosition();
						NxQuat shapeOrientation(shape->getGlobalPose().M);

						static double wheelAxleRotationSum = 0;
						wheelAxleRotationSum -= wheelShape->getAxleSpeed()*0.30*timeSinceLastFrame;

						//stringstream ss; ss << aa; CUtiles::setMessage("AV " + ss.str());

						Quaternion* axisZorrection = new Quaternion
						(
							Radian(NxMath::degToRad(90.0f)),
							Vector3(0,0,1)
						);

						Quaternion* wheelAxleRotation = new Quaternion
						(
							Radian(wheelAxleRotationSum),
							Vector3(0,1,0)
						);

						Quaternion* steeringAngle = new Quaternion
						(
							Radian(wheelShape->getSteerAngle()),
							Vector3(1,0,0)
						);

						SceneNode* sceneNode = (SceneNode*)shape->userData;

						sceneNode->setPosition(CUtiles::toVector3(shapePosition));

						sceneNode->setOrientation
						( 
							CUtiles::toQuaternion(shapeOrientation) 
							* 
							*axisZorrection 
							* 
							*steeringAngle 
							* 
							*wheelAxleRotation
						);
					}
					//----------------------------------------------------

					break;
				}
				default:
				{
					break;
				}
			}
		}
	}

	//------------------------------------------------------------
}

void CVehicleEntity::disablePhysics()
{
	if(physicsActor!=NULL)
	{
		physicsActor->raiseBodyFlag(NX_BF_KINEMATIC);
	}
}

void CVehicleEntity::enablePhysics()
{
	if(physicsActor!=NULL)
	{
		physicsActor->clearBodyFlag(NX_BF_KINEMATIC);
	}
}

string CVehicleEntity::getId()
{
	return id;
}
